

<!DOCTYPE html>
<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]-->
<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]-->
<head>
  <meta charset="utf-8">
  
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  
  <title>argoverse.utils.centerline_utils &mdash; argoverse  documentation</title>
  

  
  
  
  

  
  <script type="text/javascript" src="../../../_static/js/modernizr.min.js"></script>
  
    
      <script type="text/javascript" id="documentation_options" data-url_root="../../../" src="../../../_static/documentation_options.js"></script>
        <script type="text/javascript" src="../../../_static/jquery.js"></script>
        <script type="text/javascript" src="../../../_static/underscore.js"></script>
        <script type="text/javascript" src="../../../_static/doctools.js"></script>
        <script type="text/javascript" src="../../../_static/language_data.js"></script>
    
    <script type="text/javascript" src="../../../_static/js/theme.js"></script>

    

  
  <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
  <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
  <link rel="stylesheet" href="../../../_static/graphviz.css" type="text/css" />
    <link rel="index" title="Index" href="../../../genindex.html" />
    <link rel="search" title="Search" href="../../../search.html" /> 
</head>

<body class="wy-body-for-nav">

   
  <div class="wy-grid-for-nav">
    
    <nav data-toggle="wy-nav-shift" class="wy-nav-side">
      <div class="wy-side-scroll">
        <div class="wy-side-nav-search" >
          

          
            <a href="../../../index.html" class="icon icon-home"> argoverse
          

          
          </a>

          
            
            
          

          
<div role="search">
  <form id="rtd-search-form" class="wy-form" action="../../../search.html" method="get">
    <input type="text" name="q" placeholder="Search docs" />
    <input type="hidden" name="check_keywords" value="yes" />
    <input type="hidden" name="area" value="default" />
  </form>
</div>

          
        </div>

        <div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
          
            
            
              
            
            
              <!-- Local TOC -->
              <div class="local-toc"></div>
            
          
        </div>
      </div>
    </nav>

    <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">

      
      <nav class="wy-nav-top" aria-label="top navigation">
        
          <i data-toggle="wy-nav-top" class="fa fa-bars"></i>
          <a href="../../../index.html">argoverse</a>
        
      </nav>


      <div class="wy-nav-content">
        
        <div class="rst-content">
        
          















<div role="navigation" aria-label="breadcrumbs navigation">

  <ul class="wy-breadcrumbs">
    
      <li><a href="../../../index.html">Docs</a> &raquo;</li>
        
          <li><a href="../../index.html">Module code</a> &raquo;</li>
        
      <li>argoverse.utils.centerline_utils</li>
    
    
      <li class="wy-breadcrumbs-aside">
        
      </li>
    
  </ul>

  
  <hr/>
</div>
          <div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
           <div itemprop="articleBody">
            
  <h1>Source code for argoverse.utils.centerline_utils</h1><div class="highlight"><pre>
<span></span><span class="c1"># &lt;Copyright 2019, Argo AI, LLC. Released under the MIT license.&gt;</span>

<span class="kn">import</span> <span class="nn">datetime</span>
<span class="kn">import</span> <span class="nn">math</span>
<span class="kn">from</span> <span class="nn">typing</span> <span class="k">import</span> <span class="n">Iterable</span><span class="p">,</span> <span class="n">List</span><span class="p">,</span> <span class="n">Sequence</span><span class="p">,</span> <span class="n">Set</span><span class="p">,</span> <span class="n">Tuple</span>

<span class="kn">import</span> <span class="nn">matplotlib.pyplot</span> <span class="k">as</span> <span class="nn">plt</span>
<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">from</span> <span class="nn">argoverse.map_representation.lane_segment</span> <span class="k">import</span> <span class="n">LaneSegment</span>
<span class="kn">from</span> <span class="nn">shapely.geometry</span> <span class="k">import</span> <span class="n">LinearRing</span><span class="p">,</span> <span class="n">LineString</span><span class="p">,</span> <span class="n">Point</span><span class="p">,</span> <span class="n">Polygon</span>

<span class="kn">from</span> <span class="nn">.</span> <span class="k">import</span> <span class="n">mpl_plotting_utils</span>
<span class="kn">from</span> <span class="nn">.interpolate</span> <span class="k">import</span> <span class="n">interp_arc</span>


<div class="viewcode-block" id="swap_left_and_right"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.swap_left_and_right">[docs]</a><span class="k">def</span> <span class="nf">swap_left_and_right</span><span class="p">(</span>
    <span class="n">condition</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">left_centerline</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">right_centerline</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span>
<span class="p">)</span> <span class="o">-&gt;</span> <span class="n">Iterable</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">]:</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Swap points in left and right centerline according to condition.</span>

<span class="sd">    Args:</span>
<span class="sd">       condition: Numpy array of shape (N,) of type boolean. Where true, swap the values in the left and</span>
<span class="sd">                   right centerlines.</span>
<span class="sd">       left_centerline: The left centerline, whose points should be swapped with the right centerline.</span>
<span class="sd">       right_centerline: The right centerline.</span>

<span class="sd">    Returns:</span>
<span class="sd">       left_centerline</span>
<span class="sd">       right_centerline</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">right_swap_indices</span> <span class="o">=</span> <span class="n">right_centerline</span><span class="p">[</span><span class="n">condition</span><span class="p">]</span>
    <span class="n">left_swap_indices</span> <span class="o">=</span> <span class="n">left_centerline</span><span class="p">[</span><span class="n">condition</span><span class="p">]</span>

    <span class="n">left_centerline</span><span class="p">[</span><span class="n">condition</span><span class="p">]</span> <span class="o">=</span> <span class="n">right_swap_indices</span>
    <span class="n">right_centerline</span><span class="p">[</span><span class="n">condition</span><span class="p">]</span> <span class="o">=</span> <span class="n">left_swap_indices</span>
    <span class="k">return</span> <span class="n">left_centerline</span><span class="p">,</span> <span class="n">right_centerline</span></div>


<div class="viewcode-block" id="centerline_to_polygon"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.centerline_to_polygon">[docs]</a><span class="k">def</span> <span class="nf">centerline_to_polygon</span><span class="p">(</span>
    <span class="n">centerline</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">width_scaling_factor</span><span class="p">:</span> <span class="nb">float</span> <span class="o">=</span> <span class="mf">1.0</span><span class="p">,</span> <span class="n">visualize</span><span class="p">:</span> <span class="nb">bool</span> <span class="o">=</span> <span class="kc">False</span>
<span class="p">)</span> <span class="o">-&gt;</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">:</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Convert a lane centerline polyline into a rough polygon of the lane&#39;s area.</span>

<span class="sd">    On average, a lane is 3.8 meters in width. Thus, we allow 1.9 m on each side.</span>
<span class="sd">    We use this as the length of the hypotenuse of a right triangle, and compute the</span>
<span class="sd">    other two legs to find the scaled x and y displacement.</span>

<span class="sd">    Args:</span>
<span class="sd">       centerline: Numpy array of shape (N,2).</span>
<span class="sd">       width_scaling_factor: Multiplier that scales 3.8 meters to get the lane width.</span>
<span class="sd">       visualize: Save a figure showing the the output polygon.</span>

<span class="sd">    Returns:</span>
<span class="sd">       polygon: Numpy array of shape (2N+1,2), with duplicate first and last vertices.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="c1"># eliminate duplicates</span>
    <span class="n">_</span><span class="p">,</span> <span class="n">inds</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">unique</span><span class="p">(</span><span class="n">centerline</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="mi">0</span><span class="p">,</span> <span class="n">return_index</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span>
    <span class="c1"># does not return indices in sorted order</span>
    <span class="n">inds</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">sort</span><span class="p">(</span><span class="n">inds</span><span class="p">)</span>
    <span class="n">centerline</span> <span class="o">=</span> <span class="n">centerline</span><span class="p">[</span><span class="n">inds</span><span class="p">]</span>

    <span class="n">dx</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">gradient</span><span class="p">(</span><span class="n">centerline</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">])</span>
    <span class="n">dy</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">gradient</span><span class="p">(</span><span class="n">centerline</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">])</span>

    <span class="c1"># compute the normal at each point</span>
    <span class="n">slopes</span> <span class="o">=</span> <span class="n">dy</span> <span class="o">/</span> <span class="n">dx</span>
    <span class="n">inv_slopes</span> <span class="o">=</span> <span class="o">-</span><span class="mf">1.0</span> <span class="o">/</span> <span class="n">slopes</span>

    <span class="n">thetas</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">arctan</span><span class="p">(</span><span class="n">inv_slopes</span><span class="p">)</span>
    <span class="n">x_disp</span> <span class="o">=</span> <span class="mf">3.8</span> <span class="o">*</span> <span class="n">width_scaling_factor</span> <span class="o">/</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">thetas</span><span class="p">)</span>
    <span class="n">y_disp</span> <span class="o">=</span> <span class="mf">3.8</span> <span class="o">*</span> <span class="n">width_scaling_factor</span> <span class="o">/</span> <span class="mf">2.0</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">thetas</span><span class="p">)</span>

    <span class="n">displacement</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="n">x_disp</span><span class="p">[:,</span> <span class="n">np</span><span class="o">.</span><span class="n">newaxis</span><span class="p">],</span> <span class="n">y_disp</span><span class="p">[:,</span> <span class="n">np</span><span class="o">.</span><span class="n">newaxis</span><span class="p">]])</span>
    <span class="n">right_centerline</span> <span class="o">=</span> <span class="n">centerline</span> <span class="o">+</span> <span class="n">displacement</span>
    <span class="n">left_centerline</span> <span class="o">=</span> <span class="n">centerline</span> <span class="o">-</span> <span class="n">displacement</span>

    <span class="c1"># right centerline position depends on sign of dx and dy</span>
    <span class="n">subtract_cond1</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">logical_and</span><span class="p">(</span><span class="n">dx</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">,</span> <span class="n">dy</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">)</span>
    <span class="n">subtract_cond2</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">logical_and</span><span class="p">(</span><span class="n">dx</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">,</span> <span class="n">dy</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">)</span>
    <span class="n">add_cond1</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">logical_and</span><span class="p">(</span><span class="n">dx</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">,</span> <span class="n">dy</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">)</span>
    <span class="n">add_cond2</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">logical_and</span><span class="p">(</span><span class="n">dx</span> <span class="o">&lt;</span> <span class="mi">0</span><span class="p">,</span> <span class="n">dy</span> <span class="o">&gt;</span> <span class="mi">0</span><span class="p">)</span>
    <span class="n">subtract_cond</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">logical_or</span><span class="p">(</span><span class="n">subtract_cond1</span><span class="p">,</span> <span class="n">subtract_cond2</span><span class="p">)</span>
    <span class="n">add_cond</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">logical_or</span><span class="p">(</span><span class="n">add_cond1</span><span class="p">,</span> <span class="n">add_cond2</span><span class="p">)</span>
    <span class="n">left_centerline</span><span class="p">,</span> <span class="n">right_centerline</span> <span class="o">=</span> <span class="n">swap_left_and_right</span><span class="p">(</span><span class="n">subtract_cond</span><span class="p">,</span> <span class="n">left_centerline</span><span class="p">,</span> <span class="n">right_centerline</span><span class="p">)</span>

    <span class="c1"># right centerline also depended on if we added or subtracted y</span>
    <span class="n">neg_disp_cond</span> <span class="o">=</span> <span class="n">displacement</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">&gt;</span> <span class="mi">0</span>
    <span class="n">left_centerline</span><span class="p">,</span> <span class="n">right_centerline</span> <span class="o">=</span> <span class="n">swap_left_and_right</span><span class="p">(</span><span class="n">neg_disp_cond</span><span class="p">,</span> <span class="n">left_centerline</span><span class="p">,</span> <span class="n">right_centerline</span><span class="p">)</span>

    <span class="k">if</span> <span class="n">visualize</span><span class="p">:</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">scatter</span><span class="p">(</span><span class="n">centerline</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">centerline</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="mi">20</span><span class="p">,</span> <span class="n">marker</span><span class="o">=</span><span class="s2">&quot;.&quot;</span><span class="p">,</span> <span class="n">color</span><span class="o">=</span><span class="s2">&quot;b&quot;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">scatter</span><span class="p">(</span><span class="n">right_centerline</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">right_centerline</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="mi">20</span><span class="p">,</span> <span class="n">marker</span><span class="o">=</span><span class="s2">&quot;.&quot;</span><span class="p">,</span> <span class="n">color</span><span class="o">=</span><span class="s2">&quot;r&quot;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">scatter</span><span class="p">(</span><span class="n">left_centerline</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">left_centerline</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="mi">20</span><span class="p">,</span> <span class="n">marker</span><span class="o">=</span><span class="s2">&quot;.&quot;</span><span class="p">,</span> <span class="n">color</span><span class="o">=</span><span class="s2">&quot;g&quot;</span><span class="p">)</span>
        <span class="n">fname</span> <span class="o">=</span> <span class="n">datetime</span><span class="o">.</span><span class="n">datetime</span><span class="o">.</span><span class="n">utcnow</span><span class="p">()</span><span class="o">.</span><span class="n">strftime</span><span class="p">(</span><span class="s2">&quot;%Y_%m_</span><span class="si">%d</span><span class="s2">_%H_%M_%S_</span><span class="si">%f</span><span class="s2">&quot;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">savefig</span><span class="p">(</span><span class="n">f</span><span class="s2">&quot;polygon_unit_tests/</span><span class="si">{fname}</span><span class="s2">.png&quot;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">close</span><span class="p">(</span><span class="s2">&quot;all&quot;</span><span class="p">)</span>

    <span class="c1"># return the polygon</span>
    <span class="k">return</span> <span class="n">convert_lane_boundaries_to_polygon</span><span class="p">(</span><span class="n">right_centerline</span><span class="p">,</span> <span class="n">left_centerline</span><span class="p">)</span></div>


<div class="viewcode-block" id="convert_lane_boundaries_to_polygon"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.convert_lane_boundaries_to_polygon">[docs]</a><span class="k">def</span> <span class="nf">convert_lane_boundaries_to_polygon</span><span class="p">(</span><span class="n">right_lane_bounds</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">left_lane_bounds</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)</span> <span class="o">-&gt;</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">:</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Take a left and right lane boundary and make a polygon of the lane segment, closing both ends of the segment.</span>

<span class="sd">    These polygons have the last vertex repeated (that is, first vertex == last vertex).</span>

<span class="sd">    Args:</span>
<span class="sd">       right_lane_bounds: Right lane boundary points. Shape is (N, 2).</span>
<span class="sd">       left_lane_bounds: Left lane boundary points.</span>

<span class="sd">    Returns:</span>
<span class="sd">       polygon: Numpy array of shape (2N+1,2)</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="k">assert</span> <span class="n">right_lane_bounds</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">==</span> <span class="n">left_lane_bounds</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
    <span class="n">polygon</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">vstack</span><span class="p">([</span><span class="n">right_lane_bounds</span><span class="p">,</span> <span class="n">left_lane_bounds</span><span class="p">[::</span><span class="o">-</span><span class="mi">1</span><span class="p">]])</span>
    <span class="n">polygon</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">vstack</span><span class="p">([</span><span class="n">polygon</span><span class="p">,</span> <span class="n">right_lane_bounds</span><span class="p">[</span><span class="mi">0</span><span class="p">]])</span>
    <span class="k">return</span> <span class="n">polygon</span></div>


<div class="viewcode-block" id="filter_candidate_centerlines"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.filter_candidate_centerlines">[docs]</a><span class="k">def</span> <span class="nf">filter_candidate_centerlines</span><span class="p">(</span>
    <span class="n">xy</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">candidate_cl</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span> <span class="n">stationary_threshold</span><span class="p">:</span> <span class="nb">float</span> <span class="o">=</span> <span class="mf">2.0</span><span class="p">,</span> <span class="n">max_dist_margin</span><span class="p">:</span> <span class="nb">float</span> <span class="o">=</span> <span class="mf">2.0</span>
<span class="p">)</span> <span class="o">-&gt;</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">]:</span>
    <span class="sd">&quot;&quot;&quot;Filter candidate centerlines based on the distance travelled along the centerline.</span>

<span class="sd">    Args:</span>
<span class="sd">        xy: Trajectory coordinates.</span>
<span class="sd">        candidate_cl: List of candidate centerlines.</span>
<span class="sd">        stationary_threshold: minimum displacement to be called as non-stationary.</span>
<span class="sd">        max_dist_margin:</span>

<span class="sd">    Returns:</span>
<span class="sd">        filtered_candidate_centerlines: filtered list of candidate centerlines</span>

<span class="sd">    &quot;&quot;&quot;</span>

    <span class="c1"># Check if stationary</span>
    <span class="k">if</span> <span class="n">math</span><span class="o">.</span><span class="n">sqrt</span><span class="p">((</span><span class="n">xy</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">-</span> <span class="n">xy</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span> <span class="o">**</span> <span class="mi">2</span> <span class="o">+</span> <span class="p">(</span><span class="n">xy</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">-</span> <span class="n">xy</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span> <span class="o">**</span> <span class="mi">2</span><span class="p">)</span> <span class="o">&lt;</span> <span class="n">stationary_threshold</span><span class="p">:</span>
        <span class="n">stationary</span> <span class="o">=</span> <span class="kc">True</span>
    <span class="k">else</span><span class="p">:</span>
        <span class="n">stationary</span> <span class="o">=</span> <span class="kc">False</span>

    <span class="c1"># Filtering candidates to retain only those with distance along centerline close to traj length</span>
    <span class="c1"># Fit a second order polynomial and find trajectory length</span>
    <span class="n">POLY_ORDER</span> <span class="o">=</span> <span class="mi">2</span>
    <span class="n">poly</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">poly1d</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">polyfit</span><span class="p">(</span><span class="n">xy</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">xy</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">POLY_ORDER</span><span class="p">))</span>
    <span class="n">obs_y_smooth</span> <span class="o">=</span> <span class="p">[</span><span class="n">poly</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">xy</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]]</span>
    <span class="n">xy_smooth</span> <span class="o">=</span> <span class="p">[(</span><span class="n">xy</span><span class="p">[</span><span class="n">i</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">obs_y_smooth</span><span class="p">[</span><span class="n">i</span><span class="p">])</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">xy</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">])]</span>
    <span class="n">traj_len</span> <span class="o">=</span> <span class="n">LineString</span><span class="p">(</span><span class="n">xy_smooth</span><span class="p">)</span><span class="o">.</span><span class="n">length</span>

    <span class="n">filtered_candidate_centerlines</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="k">for</span> <span class="n">centerline</span> <span class="ow">in</span> <span class="n">candidate_cl</span><span class="p">:</span>

        <span class="k">if</span> <span class="n">stationary</span><span class="p">:</span>
            <span class="n">filtered_candidate_centerlines</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">centerline</span><span class="p">)</span>
        <span class="k">else</span><span class="p">:</span>
            <span class="n">centerLine</span> <span class="o">=</span> <span class="n">LineString</span><span class="p">(</span><span class="n">centerline</span><span class="p">)</span>
            <span class="n">start_dist</span> <span class="o">=</span> <span class="n">centerLine</span><span class="o">.</span><span class="n">project</span><span class="p">(</span><span class="n">Point</span><span class="p">(</span><span class="n">xy</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">xy</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]))</span>
            <span class="n">end_dist</span> <span class="o">=</span> <span class="n">centerLine</span><span class="o">.</span><span class="n">project</span><span class="p">(</span><span class="n">Point</span><span class="p">(</span><span class="n">xy</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">xy</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]))</span>

            <span class="n">dist_along_cl</span> <span class="o">=</span> <span class="n">end_dist</span> <span class="o">-</span> <span class="n">start_dist</span>
            <span class="k">if</span> <span class="n">dist_along_cl</span> <span class="o">&gt;</span> <span class="n">traj_len</span> <span class="o">-</span> <span class="n">max_dist_margin</span> <span class="ow">and</span> <span class="n">dist_along_cl</span> <span class="o">&lt;</span> <span class="n">traj_len</span> <span class="o">+</span> <span class="n">max_dist_margin</span><span class="p">:</span>
                <span class="n">filtered_candidate_centerlines</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">centerline</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">filtered_candidate_centerlines</span></div>


<div class="viewcode-block" id="is_overlapping_lane_seq"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.is_overlapping_lane_seq">[docs]</a><span class="k">def</span> <span class="nf">is_overlapping_lane_seq</span><span class="p">(</span><span class="n">lane_seq1</span><span class="p">:</span> <span class="n">Sequence</span><span class="p">[</span><span class="nb">int</span><span class="p">],</span> <span class="n">lane_seq2</span><span class="p">:</span> <span class="n">Sequence</span><span class="p">[</span><span class="nb">int</span><span class="p">])</span> <span class="o">-&gt;</span> <span class="nb">bool</span><span class="p">:</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Check if the 2 lane sequences are overlapping.</span>
<span class="sd">    Overlapping is defined as::</span>

<span class="sd">        s1------s2-----------------e1--------e2</span>

<span class="sd">    Here lane2 starts somewhere on lane 1 and ends after it, OR::</span>

<span class="sd">        s1------s2-----------------e2--------e1</span>

<span class="sd">    Here lane2 starts somewhere on lane 1 and ends before it</span>

<span class="sd">    Args:</span>
<span class="sd">        lane_seq1: list of lane ids</span>
<span class="sd">        lane_seq2: list of lane ids</span>

<span class="sd">    Returns:</span>
<span class="sd">        bool, True if the lane sequences overlap</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="k">if</span> <span class="n">lane_seq2</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="ow">in</span> <span class="n">lane_seq1</span><span class="p">[</span><span class="mi">1</span><span class="p">:]</span> <span class="ow">and</span> <span class="n">lane_seq1</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span> <span class="ow">in</span> <span class="n">lane_seq2</span><span class="p">[:</span><span class="o">-</span><span class="mi">1</span><span class="p">]:</span>
        <span class="k">return</span> <span class="kc">True</span>
    <span class="k">elif</span> <span class="nb">set</span><span class="p">(</span><span class="n">lane_seq2</span><span class="p">)</span> <span class="o">&lt;=</span> <span class="nb">set</span><span class="p">(</span><span class="n">lane_seq1</span><span class="p">):</span>
        <span class="k">return</span> <span class="kc">True</span>
    <span class="k">return</span> <span class="kc">False</span></div>


<div class="viewcode-block" id="get_normal_and_tangential_distance_point"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.get_normal_and_tangential_distance_point">[docs]</a><span class="k">def</span> <span class="nf">get_normal_and_tangential_distance_point</span><span class="p">(</span>
    <span class="n">x</span><span class="p">:</span> <span class="nb">float</span><span class="p">,</span> <span class="n">y</span><span class="p">:</span> <span class="nb">float</span><span class="p">,</span> <span class="n">centerline</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">delta</span><span class="p">:</span> <span class="nb">float</span> <span class="o">=</span> <span class="mf">0.01</span><span class="p">,</span> <span class="n">last</span><span class="p">:</span> <span class="nb">bool</span> <span class="o">=</span> <span class="kc">False</span>
<span class="p">)</span> <span class="o">-&gt;</span> <span class="n">Tuple</span><span class="p">[</span><span class="nb">float</span><span class="p">,</span> <span class="nb">float</span><span class="p">]:</span>
    <span class="sd">&quot;&quot;&quot;Get normal (offset from centerline) and tangential (distance along centerline) for the given point,</span>
<span class="sd">    along the given centerline</span>

<span class="sd">    Args:</span>
<span class="sd">        x: x-coordinate in map frame</span>
<span class="sd">        y: y-coordinate in map frame</span>
<span class="sd">        centerline: centerline along which n-t is to be computed</span>
<span class="sd">        delta: Used in computing offset direction</span>
<span class="sd">        last: True if point is the last coordinate of the trajectory</span>

<span class="sd">    Return:</span>
<span class="sd">        (tang_dist, norm_dist): tangential and normal distances</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">point</span> <span class="o">=</span> <span class="n">Point</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">y</span><span class="p">)</span>
    <span class="n">centerline_ls</span> <span class="o">=</span> <span class="n">LineString</span><span class="p">(</span><span class="n">centerline</span><span class="p">)</span>

    <span class="n">tang_dist</span> <span class="o">=</span> <span class="n">centerline_ls</span><span class="o">.</span><span class="n">project</span><span class="p">(</span><span class="n">point</span><span class="p">)</span>
    <span class="n">norm_dist</span> <span class="o">=</span> <span class="n">point</span><span class="o">.</span><span class="n">distance</span><span class="p">(</span><span class="n">centerline_ls</span><span class="p">)</span>
    <span class="n">point_on_cl</span> <span class="o">=</span> <span class="n">centerline_ls</span><span class="o">.</span><span class="n">interpolate</span><span class="p">(</span><span class="n">tang_dist</span><span class="p">)</span>

    <span class="c1"># Deal with last coordinate differently. Helped in dealing with floating point precision errors.</span>
    <span class="k">if</span> <span class="ow">not</span> <span class="n">last</span><span class="p">:</span>
        <span class="n">pt1</span> <span class="o">=</span> <span class="n">point_on_cl</span><span class="o">.</span><span class="n">coords</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">pt2</span> <span class="o">=</span> <span class="n">centerline_ls</span><span class="o">.</span><span class="n">interpolate</span><span class="p">(</span><span class="n">tang_dist</span> <span class="o">+</span> <span class="n">delta</span><span class="p">)</span><span class="o">.</span><span class="n">coords</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">pt3</span> <span class="o">=</span> <span class="n">point</span><span class="o">.</span><span class="n">coords</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>

    <span class="k">else</span><span class="p">:</span>
        <span class="n">pt1</span> <span class="o">=</span> <span class="n">centerline_ls</span><span class="o">.</span><span class="n">interpolate</span><span class="p">(</span><span class="n">tang_dist</span> <span class="o">-</span> <span class="n">delta</span><span class="p">)</span><span class="o">.</span><span class="n">coords</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">pt2</span> <span class="o">=</span> <span class="n">point_on_cl</span><span class="o">.</span><span class="n">coords</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">pt3</span> <span class="o">=</span> <span class="n">point</span><span class="o">.</span><span class="n">coords</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>

    <span class="n">lr_coords</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="n">lr_coords</span><span class="o">.</span><span class="n">extend</span><span class="p">([</span><span class="n">pt1</span><span class="p">,</span> <span class="n">pt2</span><span class="p">,</span> <span class="n">pt3</span><span class="p">])</span>
    <span class="n">lr</span> <span class="o">=</span> <span class="n">LinearRing</span><span class="p">(</span><span class="n">lr_coords</span><span class="p">)</span>

    <span class="c1"># Left positive, right negative</span>
    <span class="k">if</span> <span class="n">lr</span><span class="o">.</span><span class="n">is_ccw</span><span class="p">:</span>
        <span class="k">return</span> <span class="p">(</span><span class="n">tang_dist</span><span class="p">,</span> <span class="n">norm_dist</span><span class="p">)</span>
    <span class="k">return</span> <span class="p">(</span><span class="n">tang_dist</span><span class="p">,</span> <span class="o">-</span><span class="n">norm_dist</span><span class="p">)</span></div>


<div class="viewcode-block" id="get_nt_distance"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.get_nt_distance">[docs]</a><span class="k">def</span> <span class="nf">get_nt_distance</span><span class="p">(</span><span class="n">xy</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">centerline</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">viz</span><span class="p">:</span> <span class="nb">bool</span> <span class="o">=</span> <span class="kc">False</span><span class="p">)</span> <span class="o">-&gt;</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">:</span>
    <span class="sd">&quot;&quot;&quot;Get normal (offset from centerline) and tangential (distance along centerline) distances for the given xy trajectory,</span>
<span class="sd">    along the given centerline.</span>

<span class="sd">    Args:</span>
<span class="sd">        xy: Sequence of x,y,z coordinates.</span>
<span class="sd">        centerline: centerline along which n-t is to be computed</span>
<span class="sd">        viz: True if you want to visualize the computed centerlines.</span>

<span class="sd">    Returns:</span>
<span class="sd">        nt_distance: normal (offset from centerline) and tangential (distance along centerline) distances.</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">traj_len</span> <span class="o">=</span> <span class="n">xy</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
    <span class="n">nt_distance</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">traj_len</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span>

    <span class="n">delta_offset</span> <span class="o">=</span> <span class="mf">0.01</span>
    <span class="n">last</span> <span class="o">=</span> <span class="mi">0</span>
    <span class="n">max_dist</span><span class="p">:</span> <span class="nb">float</span> <span class="o">=</span> <span class="o">-</span><span class="mi">1</span>

    <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">traj_len</span><span class="p">):</span>
        <span class="n">tang_dist</span><span class="p">,</span> <span class="n">norm_dist</span> <span class="o">=</span> <span class="n">get_normal_and_tangential_distance_point</span><span class="p">(</span><span class="n">xy</span><span class="p">[</span><span class="n">i</span><span class="p">][</span><span class="mi">0</span><span class="p">],</span> <span class="n">xy</span><span class="p">[</span><span class="n">i</span><span class="p">][</span><span class="mi">1</span><span class="p">],</span> <span class="n">centerline</span><span class="p">,</span> <span class="n">last</span><span class="o">=</span><span class="kc">False</span><span class="p">)</span>

        <span class="c1"># Keep track of the last coordinate</span>
        <span class="k">if</span> <span class="n">tang_dist</span> <span class="o">&gt;</span> <span class="n">max_dist</span><span class="p">:</span>
            <span class="n">max_dist</span> <span class="o">=</span> <span class="n">tang_dist</span>
            <span class="n">last_x</span> <span class="o">=</span> <span class="n">xy</span><span class="p">[</span><span class="n">i</span><span class="p">][</span><span class="mi">0</span><span class="p">]</span>
            <span class="n">last_y</span> <span class="o">=</span> <span class="n">xy</span><span class="p">[</span><span class="n">i</span><span class="p">][</span><span class="mi">1</span><span class="p">]</span>
            <span class="n">last_idx</span> <span class="o">=</span> <span class="n">i</span>
        <span class="n">nt_distance</span><span class="p">[</span><span class="n">i</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">norm_dist</span>
        <span class="n">nt_distance</span><span class="p">[</span><span class="n">i</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">tang_dist</span>

    <span class="n">tang_dist</span><span class="p">,</span> <span class="n">norm_dist</span> <span class="o">=</span> <span class="n">get_normal_and_tangential_distance_point</span><span class="p">(</span><span class="n">last_x</span><span class="p">,</span> <span class="n">last_y</span><span class="p">,</span> <span class="n">centerline</span><span class="p">,</span> <span class="n">last</span><span class="o">=</span><span class="kc">True</span><span class="p">)</span>
    <span class="n">nt_distance</span><span class="p">[</span><span class="n">last_idx</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">norm_dist</span>

    <span class="k">if</span> <span class="n">viz</span><span class="p">:</span>
        <span class="n">mpl_plotting_utils</span><span class="o">.</span><span class="n">visualize_centerline</span><span class="p">(</span><span class="n">centerline</span><span class="p">)</span>

    <span class="k">return</span> <span class="n">nt_distance</span></div>


<div class="viewcode-block" id="get_oracle_from_candidate_centerlines"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.get_oracle_from_candidate_centerlines">[docs]</a><span class="k">def</span> <span class="nf">get_oracle_from_candidate_centerlines</span><span class="p">(</span><span class="n">candidate_centerlines</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">],</span> <span class="n">xy</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">)</span> <span class="o">-&gt;</span> <span class="n">LineString</span><span class="p">:</span>
    <span class="sd">&quot;&quot;&quot;Get oracle centerline from candidate centerlines. Chose based on direction of travel and maximum offset.</span>
<span class="sd">    First find the centerlines along which the distance travelled is close to maximum.</span>
<span class="sd">    If there are multiple candidates, then chose the one which has minimum max offset</span>

<span class="sd">    Args:</span>
<span class="sd">        candidate_centerlines: List of candidate centerlines</span>
<span class="sd">        xy: Trajectory coordinates</span>

<span class="sd">    Returns:</span>
<span class="sd">        oracle_centerline: Oracle centerline</span>

<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">max_offset</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="s2">&quot;inf&quot;</span><span class="p">)</span>
    <span class="n">max_dist_along_cl</span> <span class="o">=</span> <span class="o">-</span><span class="nb">float</span><span class="p">(</span><span class="s2">&quot;inf&quot;</span><span class="p">)</span>

    <span class="c1"># Chose based on distance travelled along centerline</span>
    <span class="n">oracle_centerlines</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="k">for</span> <span class="n">centerline</span> <span class="ow">in</span> <span class="n">candidate_centerlines</span><span class="p">:</span>
        <span class="n">centerLine</span> <span class="o">=</span> <span class="n">LineString</span><span class="p">(</span><span class="n">centerline</span><span class="p">)</span>
        <span class="n">start_dist</span> <span class="o">=</span> <span class="n">centerLine</span><span class="o">.</span><span class="n">project</span><span class="p">(</span><span class="n">Point</span><span class="p">(</span><span class="n">xy</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">xy</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="n">end_dist</span> <span class="o">=</span> <span class="n">centerLine</span><span class="o">.</span><span class="n">project</span><span class="p">(</span><span class="n">Point</span><span class="p">(</span><span class="n">xy</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">xy</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="n">dist_along_cl</span> <span class="o">=</span> <span class="n">end_dist</span> <span class="o">-</span> <span class="n">start_dist</span>
        <span class="k">if</span> <span class="n">dist_along_cl</span> <span class="o">&gt;</span> <span class="n">max_dist_along_cl</span> <span class="o">-</span> <span class="mi">1</span><span class="p">:</span>
            <span class="n">max_dist_along_cl</span> <span class="o">=</span> <span class="n">dist_along_cl</span>
            <span class="n">oracle_centerlines</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">centerline</span><span class="p">)</span>

    <span class="c1"># Chose based on maximum offset</span>
    <span class="n">min_of_max_offset</span> <span class="o">=</span> <span class="nb">float</span><span class="p">(</span><span class="s2">&quot;inf&quot;</span><span class="p">)</span>
    <span class="k">for</span> <span class="n">centerline</span> <span class="ow">in</span> <span class="n">oracle_centerlines</span><span class="p">:</span>
        <span class="n">max_offset</span> <span class="o">=</span> <span class="mf">0.0</span>
        <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">xy</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]):</span>
            <span class="n">offset</span> <span class="o">=</span> <span class="n">Point</span><span class="p">(</span><span class="n">xy</span><span class="p">[</span><span class="n">i</span><span class="p">])</span><span class="o">.</span><span class="n">distance</span><span class="p">(</span><span class="n">LineString</span><span class="p">(</span><span class="n">centerline</span><span class="p">))</span>
            <span class="n">max_offset</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">offset</span><span class="p">,</span> <span class="n">max_offset</span><span class="p">)</span>
        <span class="k">if</span> <span class="n">max_offset</span> <span class="o">&lt;</span> <span class="n">min_of_max_offset</span><span class="p">:</span>
            <span class="n">min_of_max_offset</span> <span class="o">=</span> <span class="n">max_offset</span>
            <span class="n">oracle_centerline</span> <span class="o">=</span> <span class="n">centerline</span>

    <span class="k">return</span> <span class="n">oracle_centerline</span></div>


<div class="viewcode-block" id="get_centerlines_most_aligned_with_trajectory"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.get_centerlines_most_aligned_with_trajectory">[docs]</a><span class="k">def</span> <span class="nf">get_centerlines_most_aligned_with_trajectory</span><span class="p">(</span><span class="n">xy</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">candidate_cl</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">])</span> <span class="o">-&gt;</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">]:</span>
    <span class="sd">&quot;&quot;&quot;Get the centerline from candidate_cl along which the trajectory travelled maximum distance</span>

<span class="sd">    Args:</span>
<span class="sd">        xy: Trajectory coordinates</span>
<span class="sd">        candidate_cl: List of candidate centerlines</span>

<span class="sd">    Returns:</span>
<span class="sd">        candidate_centerlines: centerlines along which distance travelled is maximum</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">max_dist_along_cl</span> <span class="o">=</span> <span class="o">-</span><span class="nb">float</span><span class="p">(</span><span class="s2">&quot;inf&quot;</span><span class="p">)</span>

    <span class="k">for</span> <span class="n">centerline</span> <span class="ow">in</span> <span class="n">candidate_cl</span><span class="p">:</span>
        <span class="n">centerline_linestring</span> <span class="o">=</span> <span class="n">LineString</span><span class="p">(</span><span class="n">centerline</span><span class="p">)</span>
        <span class="n">start_dist</span> <span class="o">=</span> <span class="n">centerline_linestring</span><span class="o">.</span><span class="n">project</span><span class="p">(</span><span class="n">Point</span><span class="p">(</span><span class="n">xy</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">xy</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="n">end_dist</span> <span class="o">=</span> <span class="n">centerline_linestring</span><span class="o">.</span><span class="n">project</span><span class="p">(</span><span class="n">Point</span><span class="p">(</span><span class="n">xy</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">xy</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="n">dist_along_cl</span> <span class="o">=</span> <span class="n">end_dist</span> <span class="o">-</span> <span class="n">start_dist</span>
        <span class="k">if</span> <span class="n">max_dist_along_cl</span> <span class="o">&lt;</span> <span class="o">-</span><span class="mi">100</span> <span class="ow">or</span> <span class="n">dist_along_cl</span> <span class="o">&gt;</span> <span class="n">max_dist_along_cl</span> <span class="o">+</span> <span class="mi">1</span><span class="p">:</span>
            <span class="n">max_dist_along_cl</span> <span class="o">=</span> <span class="n">dist_along_cl</span>
            <span class="n">candidate_centerlines</span> <span class="o">=</span> <span class="p">[</span><span class="n">centerline</span><span class="p">]</span>
        <span class="k">elif</span> <span class="n">dist_along_cl</span> <span class="o">&gt;</span> <span class="n">max_dist_along_cl</span> <span class="o">-</span> <span class="mi">1</span><span class="p">:</span>
            <span class="n">candidate_centerlines</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">centerline</span><span class="p">)</span>
            <span class="n">max_dist_along_cl</span> <span class="o">=</span> <span class="nb">max</span><span class="p">(</span><span class="n">max_dist_along_cl</span><span class="p">,</span> <span class="n">dist_along_cl</span><span class="p">)</span>

    <span class="k">return</span> <span class="n">candidate_centerlines</span></div>


<div class="viewcode-block" id="remove_overlapping_lane_seq"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.remove_overlapping_lane_seq">[docs]</a><span class="k">def</span> <span class="nf">remove_overlapping_lane_seq</span><span class="p">(</span><span class="n">lane_seqs</span><span class="p">:</span> <span class="n">Sequence</span><span class="p">[</span><span class="n">Sequence</span><span class="p">[</span><span class="nb">int</span><span class="p">]])</span> <span class="o">-&gt;</span> <span class="n">List</span><span class="p">[</span><span class="n">Sequence</span><span class="p">[</span><span class="nb">int</span><span class="p">]]:</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Remove lane sequences which are overlapping to some extent</span>

<span class="sd">    Args:</span>
<span class="sd">        lane_seqs (list of list of integers): List of sequence of lane ids (Eg. [[12345, 12346, 12347], [12345, 12348]])</span>

<span class="sd">    Returns:</span>
<span class="sd">        List of sequence of lane ids (e.g. ``[[12345, 12346, 12347], [12345, 12348]]``)</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">redundant_lane_idx</span><span class="p">:</span> <span class="n">Set</span><span class="p">[</span><span class="nb">int</span><span class="p">]</span> <span class="o">=</span> <span class="nb">set</span><span class="p">()</span>
    <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">lane_seqs</span><span class="p">)):</span>
        <span class="k">for</span> <span class="n">j</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">lane_seqs</span><span class="p">)):</span>
            <span class="k">if</span> <span class="n">i</span> <span class="ow">in</span> <span class="n">redundant_lane_idx</span> <span class="ow">or</span> <span class="n">i</span> <span class="o">==</span> <span class="n">j</span><span class="p">:</span>
                <span class="k">continue</span>
            <span class="k">if</span> <span class="n">is_overlapping_lane_seq</span><span class="p">(</span><span class="n">lane_seqs</span><span class="p">[</span><span class="n">i</span><span class="p">],</span> <span class="n">lane_seqs</span><span class="p">[</span><span class="n">j</span><span class="p">]):</span>
                <span class="n">redundant_lane_idx</span><span class="o">.</span><span class="n">add</span><span class="p">(</span><span class="n">j</span><span class="p">)</span>

    <span class="n">unique_lane_seqs</span> <span class="o">=</span> <span class="p">[</span><span class="n">lane_seqs</span><span class="p">[</span><span class="n">i</span><span class="p">]</span> <span class="k">for</span> <span class="n">i</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="nb">len</span><span class="p">(</span><span class="n">lane_seqs</span><span class="p">))</span> <span class="k">if</span> <span class="n">i</span> <span class="ow">not</span> <span class="ow">in</span> <span class="n">redundant_lane_idx</span><span class="p">]</span>
    <span class="k">return</span> <span class="n">unique_lane_seqs</span></div>


<div class="viewcode-block" id="lane_waypt_to_query_dist"><a class="viewcode-back" href="../../../argoverse.utils.html#argoverse.utils.centerline_utils.lane_waypt_to_query_dist">[docs]</a><span class="k">def</span> <span class="nf">lane_waypt_to_query_dist</span><span class="p">(</span>
    <span class="n">query_xy_city_coords</span><span class="p">:</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">nearby_lane_objs</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">LaneSegment</span><span class="p">]</span>
<span class="p">)</span> <span class="o">-&gt;</span> <span class="n">Tuple</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">,</span> <span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">]:</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Compute the distance from a query to the closest waypoint in nearby lanes.</span>

<span class="sd">    Args:</span>
<span class="sd">       query_xy_city_coords: Numpy array of shape (2,)</span>
<span class="sd">       nearby_lane_objs: list of LaneSegment objects</span>

<span class="sd">    Returns:</span>
<span class="sd">       Tuple of (per_lane_dists, min_dist_nn_indices, dense_centerlines); all numpy arrays</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">per_lane_dists</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="nb">float</span><span class="p">]</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="n">dense_centerlines</span><span class="p">:</span> <span class="n">List</span><span class="p">[</span><span class="n">np</span><span class="o">.</span><span class="n">ndarray</span><span class="p">]</span> <span class="o">=</span> <span class="p">[]</span>
    <span class="k">for</span> <span class="n">nn_idx</span><span class="p">,</span> <span class="n">lane_obj</span> <span class="ow">in</span> <span class="nb">enumerate</span><span class="p">(</span><span class="n">nearby_lane_objs</span><span class="p">):</span>
        <span class="n">centerline</span> <span class="o">=</span> <span class="n">lane_obj</span><span class="o">.</span><span class="n">centerline</span>
        <span class="c1"># densely sample more points</span>
        <span class="n">sample_num</span> <span class="o">=</span> <span class="mi">50</span>
        <span class="n">centerline</span> <span class="o">=</span> <span class="n">interp_arc</span><span class="p">(</span><span class="n">sample_num</span><span class="p">,</span> <span class="n">centerline</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">centerline</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">])</span>
        <span class="n">dense_centerlines</span> <span class="o">+=</span> <span class="p">[</span><span class="n">centerline</span><span class="p">]</span>
        <span class="c1"># compute norms to waypoints</span>
        <span class="n">waypoint_dist</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">centerline</span> <span class="o">-</span> <span class="n">query_xy_city_coords</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="mi">1</span><span class="p">)</span><span class="o">.</span><span class="n">min</span><span class="p">()</span>
        <span class="n">per_lane_dists</span> <span class="o">+=</span> <span class="p">[</span><span class="n">waypoint_dist</span><span class="p">]</span>
    <span class="n">per_lane_dists</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">(</span><span class="n">per_lane_dists</span><span class="p">)</span>
    <span class="n">min_dist_nn_indices</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">argsort</span><span class="p">(</span><span class="n">per_lane_dists</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">per_lane_dists</span><span class="p">,</span> <span class="n">min_dist_nn_indices</span><span class="p">,</span> <span class="n">dense_centerlines</span></div>
</pre></div>

           </div>
           
          </div>
          <footer>
  

  <hr/>

  <div role="contentinfo">
    <p>
        &copy; Copyright 2019, Argo AI, LLC

    </p>
  </div>
  Built with <a href="http://sphinx-doc.org/">Sphinx</a> using a <a href="https://github.com/rtfd/sphinx_rtd_theme">theme</a> provided by <a href="https://readthedocs.org">Read the Docs</a>. 

</footer>

        </div>
      </div>

    </section>

  </div>
  


  <script type="text/javascript">
      jQuery(function () {
          SphinxRtdTheme.Navigation.enable(true);
      });
  </script>

  
  
    
   

</body>
</html>